#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from copy import deepcopy


def Init_State():
    # 初始化move_group的API
    moveit_commander.roscpp_initialize(sys.argv)
    # 初始化ROS节点
    rospy.init_node('moveit_cartesian_demo', anonymous=True)              
    # 初始化需要使用move group控制的机械臂中的arm group
    arm = MoveGroupCommander('gluon')
    
    # 当运动规划失败后，允许重新规划
    arm.allow_replanning(True)
    # 设置目标位置所使用的参考坐标系
    arm.set_pose_reference_frame('base_link')      
    # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.set_goal_position_tolerance(0.01)
    arm.set_goal_orientation_tolerance(0.1)

    # 控制机械臂运动到之前设置的“home”姿态
    arm.set_named_target('home_pose')
    arm.go()

    # 关闭并退出moveit
    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)

def Move(target_pose):
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('moveit_cartesian_demo', anonymous=True)              
    arm = MoveGroupCommander('gluon')
    arm.allow_replanning(True)
    arm.set_pose_reference_frame('base_link')      
    arm.set_goal_position_tolerance(0.01)
    arm.set_goal_orientation_tolerance(0.1)
    end_effector_link = arm.get_end_effector_link()

    # 设置机器臂当前的状态作为运动初始状态
    arm.set_start_state_to_current_state()
    # 设置机械臂终端运动的目标位姿
    arm.set_pose_target(target_pose, end_effector_link)
    # 规划运动路径
    plan_success,traj,planning_time,error_code = arm.plan()
    # 按照规划的运动路径控制机械臂运动
    arm.execute(traj)
    rospy.sleep(1)

    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)

def Movebysome(target_pose,interval_num = 3):
    '''
    笛卡尔运动规划
    : target pose 末端执行器目标位置
    : interval_num 间隔数目
    '''
    # 初始化move_group的API
    moveit_commander.roscpp_initialize(sys.argv)
    # 初始化ROS节点
    rospy.init_node('moveit_cartesian_demo', anonymous=True)              
    # 初始化需要使用move group控制的机械臂中的arm group
    arm = MoveGroupCommander('gluon')
    
    # 当运动规划失败后，允许重新规划
    arm.allow_replanning(True)
    # 设置目标位置所使用的参考坐标系
    arm.set_pose_reference_frame('base_link')      
    # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.set_goal_position_tolerance(0.01)
    arm.set_goal_orientation_tolerance(0.1)
    
    # 获取终端link的名称和起始位姿
    end_effector_link = arm.get_end_effector_link()

    #初始化位姿
    # start_pose = arm.get_current_pose(end_effector_link).pose                               
    # print(start_pose)
    start_pose = Pose()
    start_pose.orientation.y = 0.70711
    start_pose.orientation.w = 0.70711
    start_pose.position.x = 0.30
    start_pose.position.y = 0.0
    start_pose.position.z = 0.10
     # 设置机械臂终端运动的目标位姿
    arm.set_pose_target(target_pose, end_effector_link)
    # 规划运动路径
    plan_success,traj,planning_time,error_code = arm.plan()
    # 按照规划的运动路径控制机械臂运动
    arm.execute(traj)
    rospy.sleep(1)


    # 中间位姿  从start_pose 到 target_pose
    wpose = deepcopy(start_pose)
    waypoints = []

    ## 设置路点    
    interval_x = (target_pose.position.x - start_pose.position.x)/interval_num
    interval_y = (target_pose.position.y - start_pose.position.y)/interval_num
    interval_z = (target_pose.position.z - start_pose.position.z)/interval_num

    for i in range(interval_num):
        wpose.position.x += interval_x
        wpose.position.y += interval_y
        wpose.position.z += interval_z
        print(wpose)
        waypoints.append(wpose)


    fraction = 0.0   #路径规划覆盖率
    maxtries = 100   #最大尝试规划次数
    attempts = 0     #已经尝试规划次数
    
    # 设置机器臂当前的状态作为运动初始状态
    arm.set_start_state_to_current_state()

    # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
    while fraction < 1.0 and attempts < maxtries:
        (plan, fraction) = arm.compute_cartesian_path (waypoints, 0.01, 0.0, True)         
        attempts += 1      
        if attempts % 10 == 0:
            rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                        
    # 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
    if fraction == 1.0:
        rospy.loginfo("Path computed successfully. Moving the arm.")
        arm.execute(plan)
        rospy.loginfo("Path execution complete.")
    # 如果路径规划失败，则打印失败信息
    else:
        rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

    rospy.sleep(1)
    
    # 关闭并退出moveit
    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)
